#include <vector>
#include <iostream>
#include <fstream>
#include<string>  
#include <sstream>
using namespace std;

int main()
{
    vector<int> num;
    vector<float> x,y;
    ifstream csvInput;
    csvInput.open("/home/lin/catkin_ws/src/ground_world/worlds/world_generate/skidpad_cones.txt");//自定义桩桶点文件路径
    string oneline;
	while(getline(csvInput,oneline)){
		istringstream  readstr(oneline);
		string part0fstr;
        getline(readstr , part0fstr , ' ');
        num.push_back(atoi( part0fstr.c_str() ));
		getline(readstr , part0fstr , ' ');
		x.push_back(atof( part0fstr.c_str() ));
		getline(readstr , part0fstr , ' ');
		y.push_back(atof( part0fstr.c_str() ));
	}
	csvInput.close();
    cout << "num:" << num.size() <<endl;
    ofstream outFile;
    outFile.open("/home/lin/catkin_ws/src/ground_world/worlds/world_generate/world_generate.world");//自定义world文件生成路径
		    if(!outFile.is_open()) cout << "openfile error!" << endl;
		    outFile.setf(ios::fixed, ios::floatfield);
		    outFile.setf(ios::showpoint);

    outFile << "<?xml version='1.0'?>\n<sdf version='1.6'>\n\n<world name=\"default\">\n\n    <!-- A ground plane -->\n    <include> \n      <uri>model://ground_plane</uri>\n    </include>\n      <light name='user_directional_light_0' type='directional'>\n      <pose frame=''>-0.038318 0.048709 1 0 -0 0</pose>\n      <diffuse>0.5 0.5 0.5 1</diffuse>\n      <specular>0.1 0.1 0.1 1</specular>\n      <direction>0.1 0.1 -0.9</direction>\n      <attenuation>\n        <range>20</range>\n        <constant>0.5</constant>\n        <linear>0.01</linear>\n        <quadratic>0.001</quadratic>\n      </attenuation>\n      <cast_shadows>1</cast_shadows>\n    </light>\n\n        <physics name='default_physics' default='0' type='ode'>\n      <max_step_size>0.001</max_step_size>\n      <real_time_factor>1</real_time_factor>\n      <real_time_update_rate>1000</real_time_update_rate>\n    </physics>\n" <<endl;

    for (int i = 0; i < num.size(); i++){
        if(num[i]==1){
        outFile << "<model name='r_ztong" << i << "'>\n<link name='link_2'>\n <inertial>\n <mass>1</mass>\n<inertia>\n <ixx>0.166667</ixx>   \n<ixy>0</ixy>\n          <ixz>0</ixz>\n          <iyy>0.166667</iyy>\n          <iyz>0</iyz>\n   <izz>0.166667</izz>\n   </inertia>\n        <pose frame=''>0 0 0 0 -0 0</pose>\n      </inertial>\n      <pose frame=''> " << x[i] << "  " << y[i] << "0 0 -0 0</pose>\n      <gravity>1</gravity>\n      <self_collide>0</self_collide>\n      <kinematic>0</kinematic>\n      <enable_wind>0</enable_wind>\n      <visual name='visual'>\n        <pose frame=''>0 0 0 0 -0 0</pose>\n        <geometry>\n          <mesh>\n            <uri>/home/lin/catkin_ws/src/ground_world/models/zhuangtong1.stl</uri>\n            <scale>0.001 0.001 0.001</scale>\n          </mesh>\n        </geometry>\n        <material>\n          <lighting>1</lighting>\n          <script>\n            <uri>file://media/materials/scripts/gazebo.material</uri>\n            <name>Gazebo/Red</name>\n          </script>\n          <shader type='pixel'>\n            <normal_map>__default__</normal_map>\n          </shader>\n          <ambient>1 0 0 1</ambient>\n          <diffuse>1 0 0 1</diffuse>\n          <specular>1 0 0 1</specular>\n          <emissive>1 0 0 1</emissive>\n        </material>\n        <transparency>0</transparency>\n        <cast_shadows>1</cast_shadows>\n      </visual>\n      <collision name='collision'>\n        <laser_retro>0</laser_retro>\n        <max_contacts>10</max_contacts>\n        <pose frame=''>0 0 0 0 -0 0</pose>\n        <geometry>\n          <mesh>\n            <uri>/home/lin/catkin_ws/src/ground_world/models/zhuangtong1.stl</uri>\n            <scale>0.001 0.001 0.001</scale>\n          </mesh>\n        </geometry>\n        <surface>\n          <friction>\n            <ode>\n              <mu>1</mu>\n              <mu2>1</mu2>\n              <fdir1>0 0 0</fdir1>\n              <slip1>0</slip1>\n              <slip2>0</slip2>\n            </ode>\n            <torsional>\n              <coefficient>1</coefficient>\n              <patch_radius>0</patch_radius>\n              <surface_radius>0</surface_radius>\n              <use_patch_radius>1</use_patch_radius>\n              <ode>\n                <slip>0</slip>\n              </ode>\n            </torsional>\n          </friction>\n          <bounce>\n            <restitution_coefficient>0</restitution_coefficient>\n            <threshold>1e+06</threshold>\n          </bounce>\n          <contact>\n            <collide_without_contact>0</collide_without_contact>\n            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>\n            <collide_bitmask>1</collide_bitmask>\n            <ode>\n              <soft_cfm>0</soft_cfm>\n              <soft_erp>0.2</soft_erp>\n              <kp>1e+13</kp>\n              <kd>1</kd>\n              <max_vel>0.01</max_vel>\n              <min_depth>0</min_depth>\n            </ode>\n            <bullet>\n              <split_impulse>1</split_impulse>\n              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>\n              <soft_cfm>0</soft_cfm>\n              <soft_erp>0.2</soft_erp>\n              <kp>1e+13</kp>\n              <kd>1</kd>\n            </bullet>\n          </contact>\n        </surface>\n      </collision>\n    </link>\n    <static>0</static>\n    <allow_auto_disable>1</allow_auto_disable>\n  </model>\n\n" << endl;
        }
        else
         outFile << "<model name='b_ztong" << i << "'>\n<link name='link_2'>\n <inertial>\n <mass>1</mass>\n<inertia>\n <ixx>0.166667</ixx>   \n<ixy>0</ixy>\n          <ixz>0</ixz>\n          <iyy>0.166667</iyy>\n          <iyz>0</iyz>\n   <izz>0.166667</izz>\n   </inertia>\n        <pose frame=''>0 0 0 0 -0 0</pose>\n      </inertial>\n      <pose frame=''> " << x[i] << "  " << y[i] << "0 0 -0 0</pose>\n      <gravity>1</gravity>\n      <self_collide>0</self_collide>\n      <kinematic>0</kinematic>\n      <enable_wind>0</enable_wind>\n      <visual name='visual'>\n        <pose frame=''>0 0 0 0 -0 0</pose>\n        <geometry>\n          <mesh>\n            <uri>/home/lin/catkin_ws/src/ground_world/models/zhuangtong1.stl</uri>\n            <scale>0.001 0.001 0.001</scale>\n          </mesh>\n        </geometry>\n        <material>\n          <lighting>1</lighting>\n          <script>\n            <uri>file://media/materials/scripts/gazebo.material</uri>\n            <name>Gazebo/Blue</name>\n          </script>\n          <shader type='pixel'>\n            <normal_map>__default__</normal_map>\n          </shader>\n          <ambient> 0 0 1 1 </ambient>\n          <diffuse>0 0 1 1 </diffuse>\n          <specular> 0 0 1 1 </specular>\n          <emissive> 0 0 1 1 </emissive>\n        </material>\n        <transparency>0</transparency>\n        <cast_shadows>1</cast_shadows>\n      </visual>\n      <collision name='collision'>\n        <laser_retro>0</laser_retro>\n        <max_contacts>10</max_contacts>\n        <pose frame=''>0 0 0 0 -0 0</pose>\n        <geometry>\n          <mesh>\n            <uri>/home/lin/catkin_ws/src/ground_world/models/zhuangtong1.stl</uri>\n            <scale>0.001 0.001 0.001</scale>\n          </mesh>\n        </geometry>\n        <surface>\n          <friction>\n            <ode>\n              <mu>1</mu>\n              <mu2>1</mu2>\n              <fdir1>0 0 0</fdir1>\n              <slip1>0</slip1>\n              <slip2>0</slip2>\n            </ode>\n            <torsional>\n              <coefficient>1</coefficient>\n              <patch_radius>0</patch_radius>\n              <surface_radius>0</surface_radius>\n              <use_patch_radius>1</use_patch_radius>\n              <ode>\n                <slip>0</slip>\n              </ode>\n            </torsional>\n          </friction>\n          <bounce>\n            <restitution_coefficient>0</restitution_coefficient>\n            <threshold>1e+06</threshold>\n          </bounce>\n          <contact>\n            <collide_without_contact>0</collide_without_contact>\n            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>\n            <collide_bitmask>1</collide_bitmask>\n            <ode>\n              <soft_cfm>0</soft_cfm>\n              <soft_erp>0.2</soft_erp>\n              <kp>1e+13</kp>\n              <kd>1</kd>\n              <max_vel>0.01</max_vel>\n              <min_depth>0</min_depth>\n            </ode>\n            <bullet>\n              <split_impulse>1</split_impulse>\n              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>\n              <soft_cfm>0</soft_cfm>\n              <soft_erp>0.2</soft_erp>\n              <kp>1e+13</kp>\n              <kd>1</kd>\n            </bullet>\n          </contact>\n        </surface>\n      </collision>\n    </link>\n    <static>0</static>\n    <allow_auto_disable>1</allow_auto_disable>\n  </model>\n\n" << endl;
    }
    outFile << "</world>\n</sdf>" << endl;

    cout << "Mission accomplished!\n";
    return 0;
}
